#include "AutoFocusZigZag.h"
#include "opencv2/core.hpp"
#include <QDebug>
#include <QThread>
namespace Qly {

static double AUTOFOCUS_EPS = 0.02;
static int COUNT = 2;
AutoFocusZigZag::AutoFocusZigZag(QObject *parent)
    : AutoFocusInterface{parent}
{
    m_pWatchDog = new QTimer(this);
    m_pWatchDog->setSingleShot(true);
    connect(m_pWatchDog, &QTimer::timeout, this, &AutoFocusZigZag::on_watchdogTimeout);
}

void Qly::AutoFocusZigZag::startAutoFocus()
{
    m_count = COUNT;
    if(m_high_mm == 0.0 && m_low_mm == 0.0)
    {
        //这时不用做自动对焦。当前图像就是最佳图像
        emit autoFocusFinished(false);
        return;
    }
    m_z_mm = m_pStageControl->zTheoryPos_mm();
 //   update_zPos();

    m_current_speed_mmps = m_speed_mmps;
    m_target_up_mm = m_z_mm + m_high_mm;
    m_target_down_mm = m_z_mm + m_low_mm;

    if(m_pCamera)  m_pCamera->startGrabbing(); // 必须保证相机是采图模式
    m_state = ON_UP_TEST; // 先测试向上运动
//    qDebug() << "ON_UP_TEST, m_target_up_mm = " << m_target_up_mm << ", m_target_down_mm = " << m_target_down_mm;
    m_pStageControl->zMoveToPos(m_target_up_mm, m_current_speed_mmps);
    startWatchDog();
}

void AutoFocusZigZag::stop()
{
    m_stop = true;
    switch (m_state)
    {
    case ON_IDLE:
    case ON_STOP:
    case ON_FINISH:// 这时已经停止了。就等待发出 autoFocusFinished 信号。所以无需处理。
        break;
    case ON_UP_TEST:
        m_pStageControl->stopZ();
        m_state = ON_UP_TEST_STOP;
        break;

    case ON_MOVE_UP:
        m_pStageControl->stopZ();
        m_state = ON_MOVE_UP_STOP;
        break;

    case ON_MOVE_DOWN:
        m_pStageControl->stopZ();
        m_state = ON_MOVE_DOWN_STOP;
        break;

    case ON_MOVE_UP_STOP:
    case ON_MOVE_DOWN_STOP:
    case ON_UP_TEST_STOP:
        m_state = ON_IDLE;
        m_pWatchDog->stop();
        emit autoFocusFinished(false);
        break;
    }
}

void AutoFocusZigZag::newFocusValue(QImage &RGB24Image, double score)
{

    Q_UNUSED(RGB24Image);
//    qDebug() << "newFocusValue = " << score << "state = " << m_state;
    switch (m_state)
    {
    case ON_IDLE: // 表示没有开启自动对焦。
        old_score = score; // 更新 old_score 是为了保证 old_score 随时都是没开启对焦之前的合适的初始值。
        break;
    case ON_UP_TEST:
//        qDebug() << "score = " << score << ", old score = " << old_score << "rat = " << fabs(score - old_score) / score ;
        if( fabs(score - old_score) / score < AUTOFOCUS_EPS ) // 为了排除噪声影响，这时应该不做任何操作。这时不要更新 old_score。
        {          
            break;
        }
        if( score > old_score ) //说明当前打分比之前有明显的提升，表示方向是对的，因此切换到向上运动阶段。
        {
            old_score = score;
            m_state = ON_MOVE_UP;
//            qDebug() << "ON_UP_TEST -> ON_MOVE_UP";
            break;
        }

        // 到这里说明方向是错的，这时应该反向。
        m_state = ON_UP_TEST_STOP;// 先停下，等待停下后再反向
        m_pStageControl->stopZ();
        old_score = score;
        break;
    case ON_UP_TEST_STOP:
        old_score = score; // 在停止之前还是要更新 SCORE
        break;
    case ON_MOVE_UP:
//        qDebug() << "score = " << score << ", old score = " << old_score << "rat = " << fabs(score - old_score) / score ;
        if( fabs(score - old_score) / score < AUTOFOCUS_EPS )
        {
            // 为了排除噪声影响，这时应该不做任何操作。这时不要更新 old_score。
            break;
        }

        if( score > old_score ) //说明当前打分比之前有明显的提升，表示方向是对的。
        {
            old_score = score;
            break;
        }
        // 到这里说明该反向了。
        m_pStageControl->stopZ(); //    这里无法突然停止，
        m_state = ON_MOVE_UP_STOP;
        old_score = score;
        break;
    case ON_MOVE_UP_STOP: // 在停下之前还是要更新 old_score 的。
        old_score = score;
        break;
    case ON_MOVE_DOWN_STOP:
        old_score = score;
        break;
    case ON_MOVE_DOWN:
//        qDebug() << "ON_MOVE_DOWN: score = " << score << ", old score = " << old_score << "rat = " << fabs(score - old_score) / score ;
        if( fabs(score - old_score) / score < AUTOFOCUS_EPS )
        {
            // 为了排除噪声影响，这时应该不做任何操作。这时不要更新 old_score。
            break;
        }
        if( score > old_score ) //说明当前打分比之前有明显的提升，表示方向是对的。
        {
            old_score = score;
            break;
        }

        m_pStageControl->stopZ();
        m_state = ON_MOVE_DOWN_STOP;
        old_score = score;
        break;
    default:

        m_state = ON_IDLE;
        m_pStageControl->stopZ();
        break;
    }
}


void AutoFocusZigZag::zMoveFinished(int32_t stopcode)
{
//    m_z_mm = m_pStageControl->zTheoryPos_mm();
    switch(m_state)
    {
    case ON_IDLE:
        break;      
    case ON_UP_TEST_STOP: //到这里说明停下了，这时要反向
        m_state = ON_MOVE_DOWN;
        old_score = 0;
        m_target_up_mm = m_pStageControl->zTheoryPos_mm(); // 更新对焦区间的上界
//        qDebug() << "ON_UP_TEST_STOP -> ON_MOVE_DOWN, m_target_up_mm = " << m_target_up_mm << ", m_target_down_mm = " << m_target_down_mm;
        m_pStageControl->zMoveToPos(m_target_down_mm, m_current_speed_mmps);
        break;

    case ON_MOVE_UP_STOP: // 到这里说明有两种情况，1 可以停止了。2 继续反向搜索
        m_count --;
        if(m_count <= 0)
        {
            m_state = ON_FINISH; //这时已经停了，但是还要等其他对象也接收到 stop 信号之后才能发出 对焦结束信号。确保对焦结束信号是最后发出的。
            m_count = COUNT;
            m_pWatchDog->stop();
            QTimer::singleShot(1, this, &AutoFocusZigZag::on_finished);
            break;
        }
        // 到这里才说明要往下运动了
        m_state = ON_MOVE_DOWN;
        old_score = 0;
        m_target_up_mm = m_pStageControl->zTheoryPos_mm(); // 更新对焦区间的上界
//        qDebug() << "ON_MOVE_UP_STOP -> ON_MOVE_DOWN, m_count = " << m_count << ", target_down = " << m_target_down_mm << ", target_up = " << m_target_up_mm;
        m_current_speed_mmps = m_current_speed_mmps / 2.0; // 将速度降低一半
        m_pStageControl->zMoveToPos(m_target_down_mm, m_current_speed_mmps);
        break;

    case ON_MOVE_DOWN_STOP:
        m_count --;
        if(m_count <= 0)
        {
            m_state = ON_FINISH; //这时已经停了，但是还要等其他对象也接收到 stop 信号之后才能发出 对焦结束信号。确保对焦结束信号是最后发出的。
            m_count = COUNT;
            m_pWatchDog->stop();
            QTimer::singleShot(1, this, &AutoFocusZigZag::on_finished);
            break;
        }
        m_state = ON_MOVE_UP;
        old_score = 0;
        m_target_down_mm = m_pStageControl->zTheoryPos_mm(); // 更新对焦区间的上界
//        qDebug() << "ON_MOVE_DOWN_STOP -> ON_MOVE_UP, m_count = " << m_count << ", target_down = " << m_target_down_mm << ", target_up = " << m_target_up_mm;
        m_current_speed_mmps = m_current_speed_mmps / 2.0; // 将速度降低一半
        m_pStageControl->zMoveToPos(m_target_up_mm, m_current_speed_mmps);
        break;

    case ON_FINISH:
        m_pWatchDog->stop();
        qDebug() << "AutoFocusZigZag: emit autoFocusFinished(true);";
        emit autoFocusFinished(true);
        m_state = ON_IDLE;
        break;

    case ON_STOP:
        m_state = ON_IDLE;
        emit autoFocusFinished(false);
        break;
    case ON_UP_TEST: // 这三种情况都不应该出现
    case ON_MOVE_UP: //
    case ON_MOVE_DOWN:
        m_state = ON_IDLE;
        m_pWatchDog->stop();
        qDebug() << "AutoFocusZigZag: emit autoFocusFinished(false);";
        emit autoFocusFinished(false);
        break;
    }
}

void AutoFocusZigZag::on_watchdogTimeout()
{
    qDebug() << "AutoFocusZigZag::on_watchdogTimeout()";
    m_pStageControl->stopZ();
    m_state = ON_STOP;
}

void AutoFocusZigZag::on_finished()
{
    if(m_state == ON_FINISH)
    {
        m_state = ON_IDLE;
        emit autoFocusFinished(true);
    }

}

void AutoFocusZigZag::on_stop()
{

}

void AutoFocusZigZag::startWatchDog(int ms)
{
    if(ms <= 0)
    {
        m_pWatchDog->start(6000);
        return;
    }
    m_pWatchDog->start(ms);
}

}

